#ifndef SD_TRANSFORM_HPP
#define SD_TRANSFORM_HPP

#include "config.hpp"
#include "Eigen/Geometry"
#include "Eigen/SVD"

namespace MT {

// frame = a 3D isotropic Catersian coordinate system
// FrameTransformation = transformation from a base frame to another frame
//
// Suppose we are in a base frame L0. This structure defines a transformation to
// transform L0 to a frame L1 by using 3 standard transformations:
//   - translation = a 3D vector in L0 that points to L1's origin
//   - rotation = a [normalised] quaternion representing the rotation from L0 to L1
//   - scale = a *positive* real number telling how big L1 is compared to L0
//       == 1: same size
//        > 1: L1 is bigger
//        < 1: L1 is smaller
// In short, we assume that if a point P seen in L0 as P0 and in L1 as P1 then:
//     P0 = S R P1 + T
//   where S = scale, R = rotation matrix, T = translation

// Note that we are transforming a frame L0 to L1. It is equivalent to
// transforming points from frame L1 to frame L0.

// There are 3 frames to work with in the project: the world coordinate system, the
// local frame of a feature, and the local frame of an object (originated at its
// center point).

template<typename _Scalar>
struct FrameTransformation
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    Eigen::Matrix<_Scalar, 8, 1> A;

    typedef Eigen::Matrix<_Scalar, 3, 1> Vec3;
    typedef Eigen::Quaternion<_Scalar> Quat;

    //! translation
    inline Eigen::Map<Vec3, Eigen::Aligned> T() const
        { return Eigen::Map<Vec3, Eigen::Aligned>(&A[4]); }

    //! rotation as quaternion
    inline Eigen::Map<Quat, Eigen::Aligned> Q() const
        { return Eigen::Map<Quat, Eigen::Aligned>(&A[0]); }

    //! scaling
    inline _Scalar const &S() const { return A[7]; }
    inline _Scalar &S() { return A[7]; }

#ifdef USE_BOOST_SERIALIZATION
    //! boost::serialization
    friend class boost::serialization::access;
    template<class Archive>
    inline void serialize(Archive & ar, const unsigned int version)
	{
		ar & BOOST_SERIALIZATION_NVP(A);
	}
#endif

    FrameTransformation(FrameTransformation const &inst) { *this = inst; }
    FrameTransformation() { A.fill(0); S() = 1.0; Q().w() = 1; }
    FrameTransformation(Quat const &_Q, Vec3 const &_T, _Scalar _S=1) { Q() = _Q; T() = _T; S() = _S; }

    inline FrameTransformation &operator=(FrameTransformation const &inst) { A = inst.A; return *this; }

    // converts a point from L0 to L1
    inline Vec3 convert_forward(Vec3 const &pt) const { return (Q().inverse())*(pt-T()) / S(); }

    // converts a point from L1 to L0
    inline Vec3 convert_backward(Vec3 const &pt) const { return Q()*(pt*S())+T(); }

    // converts points from L0 to L1
    inline void convert_forward(Eigen::Matrix<_Scalar, 3, Eigen::Dynamic> const &L0_points, Eigen::Matrix<_Scalar, 3, Eigen::Dynamic> &L1_points) const
    {
        L1_points = (Q().inverse().toRotationMatrix()/S())*(L0_points.colwise()-T());
    }

    // converts points from L1 to L0
    inline void convert_backward(Eigen::Matrix<_Scalar, 3, Eigen::Dynamic> const &L1_points, Eigen::Matrix<_Scalar, 3, Eigen::Dynamic> &L0_points) const
    {
        L0_points.noalias() = ((Q().toRotationMatrix()*S())*L1_points).colwise()+T();
    }

    // concatenate the current FrameTransformation with a second FrameTransformation
    //   this: L0->L1
    //   inst: L1->L2
    //   returns a FrameTransformation representing L0->L2
    inline FrameTransformation operator*(FrameTransformation const &inst) const
    {
        return FrameTransformation(Q()*inst.Q(), convert_backward(inst.T()), S()*inst.S());
    }

    // invert the current FrameTransformation
    //   this: L0->L1
    //   returns: L1->L0
    inline FrameTransformation inverse(void) const
    {
		return FrameTransformation(Q().inverse(), convert_forward(Vec3::Zero()), 1/S());
    }

	// get translation
	inline Eigen::Translation<_Scalar, 3> get_translation() const
	{
		return Eigen::Translation<_Scalar, 3>(T());
	}

	// get scaling
	inline Eigen::UniformScaling<_Scalar> get_scaling() const
	{
		return Eigen::UniformScaling<_Scalar>(S());
	}

	// get quaternion
	inline Eigen::Quaternion<_Scalar> get_quaternion() const
	{
		return Q();
	}

	// get affine transformation
	inline Eigen::Transform<_Scalar, 3, Eigen::Affine> get_affine_transformation() const
	{
		return Eigen::Transform<_Scalar, 3, Eigen::Affine>(
			get_translation()*get_quaternion()*get_scaling());
	}
};


// get FrameTransformation from a 8D vector -- translation, quaternion, scale
template<typename _Scalar>
FrameTransformation<_Scalar> get_frame(float32 const *data)
{
    FrameTransformation<_Scalar> f;
    f.A << data[4], data[5], data[6], data[7], // rotation as quaternion
        data[0], data[1], data[2], // translation
        data[3]; // scaling
	f.Q().normalize();
    return f;
}

// set FrameTransformation to a 8D vector -- translation, quaternion, scale
template<typename _Scalar>
void set_frame(FrameTransformation<_Scalar> const &f, float32 *data)
{
    _Scalar const *dsrc = f.A.data();
    data[0] = (float32)dsrc[4]; // translation
    data[1] = (float32)dsrc[5];
    data[2] = (float32)dsrc[6];
    data[3] = (float32)dsrc[7]; // scaling
    data[4] = (float32)dsrc[0]; // rotation as quaternion
    data[5] = (float32)dsrc[1]; 
    data[6] = (float32)dsrc[2];
    data[7] = (float32)dsrc[3];
}

/*
// get FrameTransformation from a 8D vector -- translation, quaternion, scale
template<typename _Scalar>
FrameTransformation<_Scalar> get_frame(float32 const *data)
{
    FrameTransformation<_Scalar> f;
    f.A << data[3], data[4], data[5], data[6], // rotation as quaternion
        data[0], data[1], data[2], // translation
        data[7]; // scaling
	f.Q().normalize();
    return f;
}

// set FrameTransformation to a 8D vector -- translation, quaternion, scale
template<typename _Scalar>
void set_frame(FrameTransformation<_Scalar> const &f, float32 *data)
{
    _Scalar const *dsrc = f.A.data();
    data[0] = (float32)dsrc[4]; // translation
    data[1] = (float32)dsrc[5];
    data[2] = (float32)dsrc[6];
    data[3] = (float32)dsrc[0]; // rotation as quaternion
    data[4] = (float32)dsrc[1]; 
    data[5] = (float32)dsrc[2];
    data[6] = (float32)dsrc[3];
    data[7] = (float32)dsrc[7]; // scaling
}
*/

template<typename _Scalar>
std::ostream &operator <<(std::ostream &ofs, FrameTransformation<_Scalar> const &f)
{
    ofs << "FrameTransformation(Q=(" << f.Q().coeffs().transpose();
    ofs << "), T=(" << f.T().transpose();
    ofs << "), S=" << f.S() << ")";
    return ofs;
}


// TODO: doc me
template<typename _Scalar>
FrameTransformation<_Scalar> get_frame(Eigen::Transform<_Scalar, 3, Eigen::Affine> const &T)
{
	typedef Eigen::Matrix<_Scalar, 3, 3> rotation_t;
	typedef Eigen::Quaternion<_Scalar> quaternion_t;
	typedef Eigen::Matrix<_Scalar, 3, 3> scaling_t;
	rotation_t r;
	quaternion_t q;
	scaling_t s;

	//  T = translation*quaternion*scaling
	T.computeRotationScaling(&r, &s);
	q = r;

	// TODO: check whether T = t*q*s, where t = T.translation()

	FrameTransformation<_Scalar> f;
	f.Q() = q;
	f.S() = s.coeff(0,0); // TODO: check s
	f.T() = T.translation();
	return f; // TODO: check if this f is correct
}


} // namespace MT


#endif
